// QUIRKSIM
// Copyright (c) 2009 Gregory Andrew Robinson (garobinson@ucdavis.edu)
//
// Permission is hereby granted, free of charge, to any person
// obtaining a copy of this software and associated documentation
// files (the "Software"), to deal in the Software without
// restriction, including without limitation the rights to use,
// copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following
// conditions:
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
// OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
// WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
// OTHER DEALINGS IN THE SOFTWARE.

#ifndef QUIRKSIM_TEST_CC_
#define QUIRKSIM_TEST_CC_

#include "CLHEP/Vector/ThreeVector.h"
#include <gtest/gtest.h>
#include "quirksim.h"

using ::CLHEP::Hep3Vector;

testing::AssertionResult AssertHep3VectorEq(const char* v1_expr,
                                            const char* v2_expr,
                                            const char* eps_expr,
                                            Hep3Vector expected,
                                            Hep3Vector actual,
                                            double epsilon) {
  if (expected.isNear(actual, epsilon)) {
    return ::testing::AssertionSuccess();
  }
  ::testing::Message msg;
  msg << v1_expr << " and " << v2_expr << " (<" << expected.x() << ", "
      << expected.y() << ", " << expected.z() << ">" << " and <" << actual.x()
      << ", " << actual.y() << ", " << actual.z()
      << ">) differ by " << (actual - expected).mag() << ".\n";
  return ::testing::AssertionFailure(msg);
}

class XPPointTest : public testing::Test {
 protected:
  virtual void SetUp() {
    x_i_ = Hep3Vector(42.0, -42.0, 3.14159265);
    p_i_ = Hep3Vector(2.71828183, -2.71828183, 172.5);
    point_ = new XPPoint(x_i_, p_i_);
  }
  virtual void TearDown() {
    delete point_;
  }

  Hep3Vector x_i_;
  Hep3Vector p_i_;
  XPPoint* point_;
};

TEST_F(XPPointTest, Constructor) {
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq,
                      x_i_,
                      point_->x(), 1e-300);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq,
                        p_i_,
                        point_->p(), 1e-300);
}

TEST_F(XPPointTest, Add) {
  XPDeriv derivs;
  derivs.dp_dt = Hep3Vector(1.0, 2.0, 3.0);
  derivs.dx_dt = Hep3Vector(4.0, 5.0, 6.0);
  double factor = 10.0;
  point_->Add(derivs, factor);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq,
                      x_i_ + factor * derivs.dx_dt,
                      point_->x(), 1e-300);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq,
                      p_i_ + factor * derivs.dp_dt,
                      point_->p(), 1e-300);
}

class QuirkTest : public testing::Test {
 protected:
  virtual void SetUp() {
    x1_i_ = Hep3Vector(1.0, 2.0, 3.0);
    x2_i_ = Hep3Vector(2.0, 3.0, 4.0);
    p1_i_ = Hep3Vector(3e8, 4e9, 5e10);
    p2_i_ = Hep3Vector(5.0, 4.0, 3.0);
    mass1_ = 42;
    mass2_ = 42;
    lambda1_ = 0;
    lambda2_ = 1;
    sigma_ = 42;
    point_separation_ = -1;
    sys_ = new QuirkSystem(sigma_, point_separation_);
    quirk1_ = new Quirk(x1_i_, p1_i_, mass1_, *sys_, lambda1_);
    quirk2_ = new Quirk(x2_i_, p2_i_, mass2_, *sys_, lambda2_);

    sys_->mutable_points()->push_back(quirk1_);
    sys_->mutable_points()->push_back(quirk2_);
  }

  virtual void TearDown() {
    // QuirkSystem will delete quirk1_ and quirk2_.
    delete sys_;
  }

  Hep3Vector x1_i_;
  Hep3Vector x2_i_;
  Hep3Vector p1_i_;
  Hep3Vector p2_i_;
  double mass1_;
  double mass2_;
  bool lambda1_;
  bool lambda2_;
  double sigma_;
  double point_separation_;
  QuirkSystem* sys_;
  Quirk* quirk1_;
  Quirk* quirk2_;
};

TEST_F(QuirkTest, Constructor) {
  double eps = 1e-300;

  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, x1_i_, quirk1_->x(), eps);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, x2_i_, quirk2_->x(), eps);

  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, p1_i_, quirk1_->p(), eps);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, p2_i_, quirk2_->p(), eps);

  EXPECT_DOUBLE_EQ(mass1_, quirk1_->mass());
  EXPECT_DOUBLE_EQ(mass2_, quirk2_->mass());

  EXPECT_EQ(sys_, &quirk1_->sys());
  EXPECT_EQ(sys_, &quirk2_->sys());

  EXPECT_EQ(lambda1_, quirk1_->lambda());
  EXPECT_EQ(lambda2_, quirk2_->lambda());
}

TEST_F(QuirkTest, Clone) {
  double eps = 1e-200;
  QuirkSystem clone_system(sigma_, point_separation_);
  Quirk* clone_quirk = static_cast<Quirk*>(quirk1_->Clone(clone_system));
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, quirk1_->x(), clone_quirk->x(), eps);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, quirk1_->p(), clone_quirk->p(), eps);
  EXPECT_DOUBLE_EQ(quirk1_->mass(), clone_quirk->mass());
  EXPECT_NE(&quirk1_->sys(), &clone_quirk->sys());
  EXPECT_EQ(quirk1_->lambda(), clone_quirk->lambda());
  delete clone_quirk;
}

TEST_F(QuirkTest, DxDt) {
  double eps = 1e-20;
  const Hep3Vector v_expected = Hep3Vector(
      0.00598078470265361525, 0.07974379603538153671, 0.99679745044226919504);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, v_expected, quirk1_->DxDt(), eps);
}

TEST_F(QuirkTest, OutwardUhat) {
  double eps = 1e-300;
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq,
      Hep3Vector(-1, -1, -1).unit(), quirk1_->OutwardUhat(), eps);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq,
      Hep3Vector(1, 1, 1).unit(), quirk2_->OutwardUhat(), eps);
}

TEST_F(QuirkTest, VPar) {
  double eps = 1e-300;
  EXPECT_PRED_FORMAT3(
      AssertHep3VectorEq,
      quirk1_->DxDt().project(Hep3Vector(-1, -1, -1).unit()),
      quirk1_->VPar(), eps);
}

TEST_F(QuirkTest, VPerp) {
  double eps = 1e-20;

  *quirk1_->mutable_x() = Hep3Vector(-1, 0, 0);
  *quirk2_->mutable_x() = Hep3Vector(1, 0, 0);

  EXPECT_PRED_FORMAT3(AssertHep3VectorEq,
      Hep3Vector(0, 0.07974379603538153671, 0.99679745044226919504),
      quirk1_->VPerp(), eps);

  *quirk1_->mutable_x() = Hep3Vector(-1, 0, 0);
  *quirk2_->mutable_x() = Hep3Vector(1, 0, 0);
  *quirk1_->mutable_p() = Hep3Vector(1, 0, 0);
  *quirk2_->mutable_p() = Hep3Vector(-1, 0, 0);

  EXPECT_PRED_FORMAT3(AssertHep3VectorEq,
      Hep3Vector(0, 0, 0),
      quirk1_->VPerp(), eps);
}

TEST_F(QuirkTest, DpDt) {
  double eps = 1e-15;
  *quirk1_->mutable_x() = Hep3Vector(-1.0, 0.0, 0.0);
  *quirk2_->mutable_x() = Hep3Vector(1.0, 0.0, 0.0);
  *quirk1_->mutable_p() = Hep3Vector(-1e6, 0, 0);
  *quirk2_->mutable_p() = Hep3Vector(1e6, 0, 0);

  EXPECT_PRED_FORMAT3(AssertHep3VectorEq,
                      sigma_ * ((quirk2_->x() - quirk1_->x()).unit()),
                      quirk1_->DpDt(), eps);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq,
                      sigma_ * ((quirk1_->x() - quirk2_->x()).unit()),
                      quirk2_->DpDt(), eps);
}

TEST_F(QuirkTest, ComputeDerivs) {
  double eps = 1e-15;
  XPDeriv deriv1;
  XPDeriv deriv2;
  quirk1_->ComputeDerivs(*sys_, sys_->points().begin(), &deriv1);
  quirk2_->ComputeDerivs(*sys_, sys_->points().end(), &deriv2);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, quirk1_->DxDt(), deriv1.dx_dt, eps);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, quirk1_->DpDt(), deriv1.dp_dt, eps);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, quirk2_->DxDt(), deriv2.dx_dt, eps);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, quirk2_->DpDt(), deriv2.dp_dt, eps);
}

class StringPointTest : public testing::Test {
 protected:
  virtual void SetUp() {
    x0_i_ = Hep3Vector(-2, 0, 0);
    x1_i_ = Hep3Vector(-1, 0, 0);
    x2_i_ = Hep3Vector(0, 0, 0);
    x3_i_ = Hep3Vector(1, 0, 0);
    x4_i_ = Hep3Vector(2.0, 0, 0);
    p0_i_ = Hep3Vector(0, 0, 0);
    p1_i_ = Hep3Vector(0, 0, 0);
    p2_i_ = Hep3Vector(0, 0, 0);
    p3_i_ = Hep3Vector(0, 0, 0);
    p4_i_ = Hep3Vector(0, 0, 0);
    mass1_ = 42;
    mass2_ = 42;
    lambda1_ = 0;
    lambda2_ = 1;
    sigma_ = 42;
    point_separation_ = -1;
    sys_ = new QuirkSystem(sigma_, point_separation_);
    point0_ = new Quirk(x0_i_, p0_i_, mass1_, *sys_, lambda1_);
    point1_ = new StringPoint(x1_i_, p1_i_, *sys_);
    point2_ = new StringPoint(x2_i_, p2_i_, *sys_);
    point3_ = new StringPoint(x3_i_, p3_i_, *sys_);
    point4_ = new Quirk(x4_i_, p4_i_, mass2_, *sys_, lambda2_);

    sys_->mutable_points()->push_back(point0_);
    sys_->mutable_points()->push_back(point1_);
    sys_->mutable_points()->push_back(point2_);
    sys_->mutable_points()->push_back(point3_);
    sys_->mutable_points()->push_back(point4_);
  }

  virtual void TearDown() {
    // QuirkSystem will delete quirk1_ and quirk2_ for us.
    delete sys_;
  }

  Hep3Vector x0_i_;
  Hep3Vector x1_i_;
  Hep3Vector x2_i_;
  Hep3Vector x3_i_;
  Hep3Vector x4_i_;
  Hep3Vector p0_i_;
  Hep3Vector p1_i_;
  Hep3Vector p2_i_;
  Hep3Vector p3_i_;
  Hep3Vector p4_i_;
  double mass1_;
  double mass2_;
  bool lambda1_;
  bool lambda2_;
  double sigma_;
  double point_separation_;
  QuirkSystem* sys_;
  Quirk* point0_;
  StringPoint* point1_;
  StringPoint* point2_;
  StringPoint* point3_;
  Quirk* point4_;
};

TEST_F(StringPointTest, Constructor) {
  double eps = 1e-300;

  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, x2_i_, point2_->x(), eps);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, x3_i_, point3_->x(), eps);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, x4_i_, point4_->x(), eps);

  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, p2_i_, point2_->p(), eps);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, p3_i_, point3_->p(), eps);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, p4_i_, point4_->p(), eps);

  EXPECT_EQ(sys_, &point2_->sys());
  EXPECT_EQ(sys_, &point3_->sys());
  EXPECT_EQ(sys_, &point4_->sys());
}

TEST_F(StringPointTest, Clone) {
  double eps = 1e-300;
  QuirkSystem clone_system(sigma_, point_separation_);
  StringPoint* clone_point
      = static_cast<StringPoint*>(point2_->Clone(clone_system));
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, point2_->x(), clone_point->x(), eps);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, point2_->p(), clone_point->p(), eps);
  EXPECT_NE(&point2_->sys(), &clone_point->sys());
  delete clone_point;
}

TEST_F(StringPointTest, ComputeDerivs) {
  double eps = 1e-16;
  XPDeriv deriv;

  point2_->mutable_x()->set(0, 1, 0);
  //sys_->mutable_points()->at(2)->mutable_x()->set(0, 1, 0);

  String_T::const_iterator point2_it(sys_->points().begin() + 2);

  point2_->ComputeDerivs(*sys_, point2_it, &deriv);

  Hep3Vector expected_dpdt=
      sqrt(1 / 10.0) * Hep3Vector(0, -1, 0);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, expected_dpdt, deriv.dp_dt, eps);

  double Pi = 1;
  sys_->mutable_points()->at(1)->mutable_p()->set(Pi, 0, 0);
  sys_->mutable_points()->at(2)->mutable_p()->set(Pi, 0, 0);
  sys_->mutable_points()->at(3)->mutable_p()->set(0, Pi, 0);

  point2_->ComputeDerivs(*sys_, point2_it, &deriv);

  expected_dpdt =
      sqrt(1.0 / (10.0 * (1 + Pi*Pi))) * Hep3Vector(0, -1, 0)
          +
      Pi*Pi * sqrt(1.0 / (8.0 * (1.0 + Pi*Pi))) * Hep3Vector(1, 0, 0);
  EXPECT_PRED_FORMAT3(
      AssertHep3VectorEq,
      expected_dpdt,
      point2_->DpDt(point2_it),
      1e-16);

  EXPECT_PRED_FORMAT3(AssertHep3VectorEq,
                      Hep3Vector(1, 0, 0) * sqrt(1.0 / (1 + Pi*Pi)),
                      deriv.dx_dt, eps);
}

TEST_F(StringPointTest, DxDt) {
  double eps = 1e-300;
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, Hep3Vector(0, 0, 0), point2_->DxDt(), eps);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, Hep3Vector(0, 0, 0), point3_->DxDt(), eps);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, Hep3Vector(0, 0, 0), point4_->DxDt(), eps);

  point2_->mutable_p()->set(0.5, -0.5, 0.5);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, Hep3Vector(1, -1, 1) * sqrt(1.0 / 7.0),
                      point2_->DxDt(), eps);
}

TEST_F(StringPointTest, DpDt) {
  double eps = 1e-16;

  String_T::const_iterator point_it(sys_->points().begin() + 1);

  EXPECT_PRED_FORMAT3(
      AssertHep3VectorEq, Hep3Vector(0, 0, 0), point1_->DpDt(point_it), eps);
  EXPECT_PRED_FORMAT3(
      AssertHep3VectorEq, Hep3Vector(0, 0, 0), point2_->DpDt(++point_it), eps);
  EXPECT_PRED_FORMAT3(
      AssertHep3VectorEq, Hep3Vector(0, 0, 0), point3_->DpDt(++point_it), eps);

  // Set point_it back to point 2.
  --point_it;

  point2_->mutable_x()->set(0, 1, 0);
  Hep3Vector expected_dpdt=
      sqrt(1 / 10.0) * Hep3Vector(0, -1, 0);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, 
                      expected_dpdt,
                      point2_->DpDt(point_it),
                      eps);

  double Pi = 1;
  sys_->mutable_points()->at(1)->mutable_p()->set(Pi, 0, 0);
  sys_->mutable_points()->at(2)->mutable_p()->set(Pi, 0, 0);
  sys_->mutable_points()->at(3)->mutable_p()->set(0, Pi, 0);

  expected_dpdt =
      sqrt(1.0 / (10.0 * (1 + Pi*Pi))) * Hep3Vector(0, -1, 0)
          +
      Pi*Pi * sqrt(1.0 / (8.0 * (1.0 + Pi*Pi))) * Hep3Vector(1, 0, 0);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq,
                      expected_dpdt,
                      point2_->DpDt(point_it),
                      eps);
}

class QuirkSystemTest : public testing::Test {
protected:
 virtual void SetUp() {
   x0_i_ = Hep3Vector(-2, 0, 0);
   x1_i_ = Hep3Vector(-1, 0, 0);
   x2_i_ = Hep3Vector(0, 0, 0);
   x3_i_ = Hep3Vector(1, 0, 0);
   x4_i_ = Hep3Vector(2.0, 0, 0);
   p0_i_ = Hep3Vector(0, 0, 0);
   p1_i_ = Hep3Vector(0, 0, 0);
   p2_i_ = Hep3Vector(0, 0, 0);
   p3_i_ = Hep3Vector(0, 0, 0);
   p4_i_ = Hep3Vector(0, 0, 0);
   mass1_ = 42;
   mass2_ = 42;
   lambda1_ = 0;
   lambda2_ = 1;
   sigma_ = 42;
   point_separation_ = -1;
   sys_ = new QuirkSystem(sigma_, point_separation_);
   point0_ = new Quirk(x0_i_, p0_i_, mass1_, *sys_, lambda1_);
   point1_ = new StringPoint(x1_i_, p1_i_, *sys_);
   point2_ = new StringPoint(x2_i_, p2_i_, *sys_);
   point3_ = new StringPoint(x3_i_, p3_i_, *sys_);
   point4_ = new Quirk(x4_i_, p4_i_, mass2_, *sys_, lambda2_);

   sys_->mutable_points()->push_back(point0_);
   sys_->mutable_points()->push_back(point1_);
   sys_->mutable_points()->push_back(point2_);
   sys_->mutable_points()->push_back(point3_);
   sys_->mutable_points()->push_back(point4_);
 }

 virtual void TearDown() {
   // QuirkSystem will delete quirk1_ and quirk2_ for us.
   delete sys_;
 }

 Hep3Vector x0_i_;
 Hep3Vector x1_i_;
 Hep3Vector x2_i_;
 Hep3Vector x3_i_;
 Hep3Vector x4_i_;
 Hep3Vector p0_i_;
 Hep3Vector p1_i_;
 Hep3Vector p2_i_;
 Hep3Vector p3_i_;
 Hep3Vector p4_i_;
 double mass1_;
 double mass2_;
 bool lambda1_;
 bool lambda2_;
 double sigma_;
 double point_separation_;
 QuirkSystem* sys_;
 Quirk* point0_;
 StringPoint* point1_;
 StringPoint* point2_;
 StringPoint* point3_;
 Quirk* point4_;
};

TEST_F(QuirkSystemTest, Clone) {
  const double eps = 1e-300;

  QuirkSystem* clone_sys = sys_->Clone();
  EXPECT_DOUBLE_EQ(sys_->sigma(), clone_sys->sigma());

  ASSERT_EQ(sys_->points().size(), clone_sys->points().size());

  std::vector<ParamPoint*>::const_iterator p_iter;
  p_iter = sys_->points().begin();
  std::vector<ParamPoint*>::const_iterator clone_p_iter;
  clone_p_iter = clone_sys->points().begin();
  while (p_iter != sys_->points().end()
         && clone_p_iter != clone_sys->points().end()) {
    EXPECT_PRED_FORMAT3(AssertHep3VectorEq, (*p_iter)->x(),
                        (*clone_p_iter)->x(), eps);
    EXPECT_PRED_FORMAT3(AssertHep3VectorEq, (*p_iter)->p(),
                        (*clone_p_iter)->p(), eps);
    // Make sure these are distinct objects.
    EXPECT_NE(*p_iter, *clone_p_iter);
    p_iter++;
    clone_p_iter++;
  }
  delete clone_sys;
}

TEST_F(QuirkSystemTest, InspectCollisions) {

}
/*
TEST_F(QuirkSystemTest, CM_Frame_Straight) {
  double eps = 1e-8;
  double dt = 1e-2;
  // This is a weird number to make the quirks "pretty close" to one another.
  double max_time = 10.95;

  for (double elapsed = dt; elapsed < max_time; elapsed += dt) {
    sys_->RK4Step(dt);

    EXPECT_PRED_FORMAT3(AssertHep3VectorEq,
                        Hep3Vector(sigma_ * elapsed, 0, 1e-300),
                        point0_->p(),
                        eps);
    EXPECT_PRED_FORMAT3(AssertHep3VectorEq,
                        Hep3Vector(- sigma_ * elapsed, 0, 1e-300),
                        point4_->p(),
                        eps);
    double d1_exact = x0_i_.x() + mass1_ / sigma_
          * (sqrt(1 + sigma_ * sigma_ / mass1_ / mass1_ * elapsed * elapsed) - 1);
    double d2_exact = x4_i_.x() - mass2_ / sigma_
          * (sqrt(1 + sigma_ * sigma_ / mass2_ / mass2_ * elapsed * elapsed) - 1);

    EXPECT_PRED_FORMAT3(AssertHep3VectorEq, Hep3Vector(d1_exact, 0, 0),
                        point0_->x(), eps) << "t = " << elapsed;
    EXPECT_PRED_FORMAT3(AssertHep3VectorEq, Hep3Vector(d2_exact, 0, 0),
                        point4_->x(), eps) << "t = " << elapsed;
  }
}*/

/*
TEST_F(QuirkSystemTest, DummyRun) {
  double dt = 1e-2;
  // This is a weird number to make the quirks "pretty close" to one another.
  double max_time = 10.95;

  for (double elapsed = dt; elapsed < max_time; elapsed += dt) {
    sys_->RK4Step(dt);

    for (std::vector<ParamPoint*>::const_iterator it = sys_->points().begin();
         it < sys_->points().end();
         it++) {
      Print((*it)->x());
      printf(", ");
    }
    printf("\n");
  }
}*/

TEST_F(QuirkSystemTest, Uhat) {
  double eps = 1e-10;
  Hep3Vector u;
  for(String_T::const_iterator point_it(sys_->points().end());
      point_it != sys_->points().end();
      ++point_it) {
    u = sys_->Uhat(point_it);
    EXPECT_PRED_FORMAT3(AssertHep3VectorEq, Hep3Vector(1, 0, 0), u, eps);
  }
}

TEST_F(QuirkSystemTest, DeltaL) {
  String_T::const_iterator point_it(sys_->points().begin() + 2);
  EXPECT_DOUBLE_EQ(2.0, sys_->DeltaL(point_it - 1, point_it + 1));
}

TEST_F(QuirkSystemTest, ComputeDerivs) {
  double eps = 1e-16;
  std::vector<XPDeriv> derivs;

  point2_->mutable_x()->set(0, 1, 0);
  //sys_->mutable_points()->at(2)->mutable_x()->set(0, 1, 0);

  String_T::const_iterator point2_it(sys_->points().begin() + 2);

  sys_->ComputeDerivs(*sys_, &derivs);

  Hep3Vector expected_dpdt =
      sqrt(1 / 10.0) * Hep3Vector(0, -1, 0);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, expected_dpdt, derivs[2].dp_dt, eps);

  double Pi = 1;
  sys_->mutable_points()->at(1)->mutable_p()->set(Pi, 0, 0);
  sys_->mutable_points()->at(2)->mutable_p()->set(Pi, 0, 0);
  sys_->mutable_points()->at(3)->mutable_p()->set(0, Pi, 0);

  sys_->ComputeDerivs(*sys_, &derivs);

  expected_dpdt =
      sqrt(1.0 / (10.0 * (1 + Pi*Pi))) * Hep3Vector(0, -1, 0)
          +
      Pi*Pi * sqrt(1.0 / (8.0 * (1.0 + Pi*Pi))) * Hep3Vector(1, 0, 0);
  EXPECT_PRED_FORMAT3(
      AssertHep3VectorEq,
      expected_dpdt,
      derivs[2].dp_dt,
      1e-16);

  EXPECT_PRED_FORMAT3(AssertHep3VectorEq,
                      Hep3Vector(1, 0, 0) * sqrt(1.0 / (1 + Pi*Pi)),
                      derivs[2].dx_dt, eps);
}


#endif  // QUIRKSIM_TEST_CC_
